#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import time


class MovePoseCommander(Node):
    def __init__(self):
        super().__init__('move_pose_commander')

        self.publisher_move_joint = self.create_publisher(Float64MultiArray, '/command_move_joint', 10)
        self.publisher_move_pose = self.create_publisher(Float64MultiArray, '/command_move_pose', 10)

    def wait_for_subscribers(self):
        # 等待所有订阅者连接成功
        self.get_logger().info("Waiting for subscribers to connect...")
        while (self.publisher_move_joint.get_subscription_count() == 0 or
               self.publisher_move_pose.get_subscription_count() == 0):
            rclpy.spin_once(self, timeout_sec=0.1)
            time.sleep(0.1)
        self.get_logger().info("All subscribers connected.")

    def send_commands(self):
        # 准备数据
        move_joint_cmd = [-1.215456, 0.4159984588623047, 0.4808502197265625,
                          -0.7959480285644531, 1.7736701965332031,
                          -0.8081560134887695, -2.131875991821289]
        move_pose_cmd = [0.0, -0.38, 0.3, -0.5, -0.5, 0.5, -0.5]

        # 发送 move_joint
        msg_joint = Float64MultiArray()
        msg_joint.data = move_joint_cmd
        self.publisher_move_joint.publish(msg_joint)
        self.get_logger().info("Published move_joint_cmd")
        time.sleep(5)

        # 发送 move_pose
        msg_pose = Float64MultiArray()
        msg_pose.data = move_pose_cmd
        self.publisher_move_pose.publish(msg_pose)
        self.get_logger().info("Published move_pose_cmd")
        time.sleep(1)


def main():
    rclpy.init()
    node = MovePoseCommander()
    try:
        node.wait_for_subscribers()
        node.send_commands()
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
